/**
 * \addtogroup uip6
 * @{
 */
/*
 * Copyright (c) 2010, Swedish Institute of Computer Science.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 * 3. Neither the name of the Institute nor the names of its contributors
 *    may be used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE INSTIT  PRINTF("time: CPU = %lu, LPM = %lu, LISTEN = %lu, TRANSMIT = %lu\n", energest_type_time(ENERGEST_TYPE_CPU), energest_type_time(ENERGEST_TYPE_LPM), energest_type_time(ENERGEST_TYPE_LISTEN), energest_type_time(ENERGEST_TYPE_TRANSMIT) );
 * UTE AND CONTRIBUTORS ``AS IS'' AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED.  IN NO EVENT SHALL THE INSTITUTE OR CONTRIBUTORS BE LIABLE
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
 * SUCH DAMAGE.
 *
 * This file is part of the Contiki operating system.
 *
 */
/**
 * \file
 *         The minrank-hysteresis objective function (OCP 1).
 *
 *         This implementation uses the estimated number of 
 *         transmissions (ETX) as the additive routing metric.
 *
 * \author Joakim Eriksson <joakime@sics.se>, Nicolas Tsiftes <nvt@sics.se>
 */
#include "net/rpl/rpl.h"
#include "net/rpl/rpl-private.h"
#include "net/neighbor-info.h"
//#include "energest.h"
#define DEBUG DEBUG_PRINT
#include "net/uip-debug.h"

static void reset(rpl_dag_t *);
static void parent_state_callback(rpl_parent_t *, int, int);
static rpl_parent_t *best_parent(rpl_parent_t *, rpl_parent_t *);
static rpl_dag_t *best_dag(rpl_dag_t *, rpl_dag_t *);
static rpl_rank_t calculate_rank(rpl_parent_t *, rpl_rank_t);
static void update_metric_container(rpl_instance_t *);
static rpl_rank_t calculate_energy();

rpl_of_t rpl_of_energy = {
    reset,
    parent_state_callback,
    best_parent,
    best_dag,
    calculate_rank,
    update_metric_container,
    calculate_energy,
    1
};

/* Reject parents that have a higher link metric than the following. */
#define MAX_LINK_METRIC			10

/* Reject parents that have a higher path cost than the following. */
#define MAX_PATH_COST			100

/*
 * The rank must differ more than 1/PARENT_SWITCH_THRESHOLD_DIV in order
 * to switch preferred parent.
 */
#define PARENT_SWITCH_THRESHOLD_DIV	2

typedef uint16_t rpl_path_metric_t;
static rpl_rank_t calc_energy;


/* static rpl_path_metric_t
calculate_path_metric(rpl_parent_t *p)
{
	if(p == NULL || (p->mc.obj.etx == 0 && p->rank > ROOT_RANK(p->dag->instance))) {
		return MAX_PATH_COST * RPL_DAG_MC_ETX_DIVISOR;
	} else {
		long etx = p->link_metric;
		etx = (etx * RPL_DAG_MC_ETX_DIVISOR) / NEIGHBOR_INFO_ETX_DIVISOR;
		switch(p->mc.type) {
		case RPL_DAG_MC_TEMP_ETX: {
			//PRINTF("RPL: Parent ");
        		//PRINT6ADDR(&p->addr);
        		//PRINTF(", Gotcha: %u, ETX-aggr: %u, ETX-PM: %u\n", (unsigned)p->mc.obj.temperature.gotcha, (unsigned)p->mc.obj.etx, (uint16_t) etx);
			return p->mc.obj.etx + (uint16_t) etx- 100*(p->mc.obj.temperature.gotcha);
		}
		default: {
			return p->mc.obj.etx + (uint16_t) etx;
		}
		}
	}
} */

static void
reset(rpl_dag_t *sag)
{
}

static void
parent_state_callback(rpl_parent_t *parent, int known, int etx)
{
}

rpl_rank_t calculate_energy()
{
  energest_flush();

#if CONTIKI_TARGET_SKY
  calc_energy = (rpl_rank_t) ((( (energest_type_time(ENERGEST_TYPE_CPU)<<1) + (energest_type_time(ENERGEST_TYPE_LPM)>>4) + (energest_type_time(ENERGEST_TYPE_LISTEN)<<4) + (energest_type_time(ENERGEST_TYPE_TRANSMIT)<<4)) <<1) >>15) ;
  //PRINTF("ENERGY: node energy = %lu\n",(unsigned long)calc_energy);
#else
  calc_energy = (rpl_rank_t) ((( (energest_type_time(ENERGEST_TYPE_CPU)<<2) + (energest_type_time(ENERGEST_TYPE_LPM)>>1) + (energest_type_time(ENERGEST_TYPE_LISTEN)<<4) + (energest_type_time(ENERGEST_TYPE_TRANSMIT)<<4)) <<1) >>15) ;
  //PRINTF("time: LISTEN = %lu, TRANSMIT = %lu\n", (energest_type_time(ENERGEST_TYPE_LISTEN)>>15), (energest_type_time(ENERGEST_TYPE_TRANSMIT)>>15) );
#endif

  return calc_energy;
}

static rpl_rank_t
calculate_rank(rpl_parent_t *p, rpl_rank_t base_rank)
{
  rpl_rank_t new_rank;
  rpl_rank_t rank_increase;
  if(p == NULL) {
    if(base_rank == 0) {
      return INFINITE_RANK;
    }
    rank_increase = 0;
  } else {
    rank_increase = calculate_energy();

    if(base_rank == 0) {
      base_rank = p->rank;
    }
  }

  if(INFINITE_RANK - base_rank < rank_increase) {
    /* Reached the maximum rank. */
    new_rank = INFINITE_RANK;
  } else {
    /* Calculate the rank based on the new rank information from DIO or
      stored otherwise. */
    new_rank = base_rank + rank_increase;
    //PRINTF("ENERGY: base rank = %lu\n",base_rank);
    //PRINTF("ENERGY: rank_increase = %lu\n",rank_increase);
    //PRINTF("ENERGY: new_rank = %lu\n",new_rank);
  }
  //PRINTF("ENERGY: node energy = %lu\n",(unsigned long)calc_energy);
  return new_rank;
}

static rpl_dag_t *
best_dag(rpl_dag_t *d1, rpl_dag_t *d2)
{
  if(d1->grounded != d2->grounded) {
    return d1->grounded ? d1 : d2;
  }

  if(d1->preference != d2->preference) {
    return d1->preference > d2->preference ? d1 : d2;
  }

  return d1->rank < d2->rank ? d1 : d2;
}

static rpl_parent_t *
best_parent(rpl_parent_t *p1, rpl_parent_t *p2)
{
  rpl_dag_t *dag;
  rpl_rank_t min_diff;
  rpl_rank_t p1_metric;
  rpl_rank_t p2_metric;

  dag = p1->dag; /* Both parents must be in the same DAG. */

  min_diff = 2;

  p1_metric = p1->rank - 100*(p1->mc.obj.temperature.gotcha);
  p2_metric = p2->rank - 100*(p2->mc.obj.temperature.gotcha);

  /* Maintain stability of the preferred parent in case of similar ranks. */
  if(p1 == dag->preferred_parent || p2 == dag->preferred_parent) {
    if(p1_metric < p2_metric + min_diff &&
        p1_metric > p2_metric - min_diff) {
      //PRINTF("RPL: MRHOF hysteresis: %u <= %u <= %u\n",
      //p2_metric - min_diff,
      //p1_metric,
      //p2_metric + min_diff);
      //printf("p1 rank %lu, p2 rank %lu\n",p1_metric,p2_metric);

      return dag->preferred_parent;
    }
  }

  return p1_metric < p2_metric ? p1 : p2;
}

static void
update_metric_container(rpl_instance_t *instance)
{
  rpl_path_metric_t path_metric;
  rpl_dag_t *dag;
#if ((RPL_DAG_MC == RPL_DAG_MC_ENERGY) || (RPL_DAG_MC_TEMP_ENERGY))
  uint8_t type;
#endif

  instance->mc.flags = RPL_DAG_MC_FLAG_P;
  instance->mc.aggr = RPL_DAG_MC_AGGR_ADDITIVE;
  instance->mc.prec = 0;

  dag = instance->current_dag;

  if (!dag->joined) {
    /* We should probably do something here */
    return;
  }

  if(dag->rank == ROOT_RANK(instance)) {
    path_metric = 0;
  } else {
    path_metric = dag->preferred_parent->rank;
    //path_metric = calculate_path_metric(dag->preferred_parent);
  }

#if RPL_DAG_MC == RPL_DAG_MC_ETX

  instance->mc.type = RPL_DAG_MC_ETX;
  instance->mc.length = sizeof(instance->mc.obj.etx);
  instance->mc.obj.etx = path_metric;

  /*PRINTF("RPL: My path ETX to the root is %u.%u\n",
	  instance->mc.obj.etx / RPL_DAG_MC_ETX_DIVISOR,
	  (instance->mc.obj.etx % RPL_DAG_MC_ETX_DIVISOR * 100) / RPL_DAG_MC_ETX_DIVISOR); */

  /*#elif RPL_DAG_MC == RPL_DAG_MC_ENERGY //--> scommentare here!

  instance->mc.type = RPL_DAG_MC_ENERGY;
  instance->mc.length = sizeof(instance->mc.obj.energy);

  if(dag->rank == ROOT_RANK(instance)) {
    type = RPL_DAG_MC_ENERGY_TYPE_MAINS;
    instance->mc.obj.energy.energy_est = 0;
  } else {
    type = RPL_DAG_MC_ENERGY_TYPE_BATTERY;


  instance->mc.obj.energy.flags = type << RPL_DAG_MC_ENERGY_TYPE;
  instance->mc.obj.energy.energy_est = (unsigned long)  (unsigned long) (((energest_type_time(ENERGEST_TYPE_CPU)<<1)+(energest_type_time(ENERGEST_TYPE_LPM)>>4)+(energest_type_time(ENERGEST_TYPE_LISTEN<<4)+ (energest_type_time(ENERGEST_TYPE_TRANSMIT)<<4)))<<1)>>15;
}*/


#elif RPL_DAG_MC == RPL_DAG_MC_TEMP_ENERGY
  instance->mc.type = RPL_DAG_MC_TEMP_ENERGY;
  instance->mc.length = sizeof(instance->mc.obj.energy);

  if(dag->rank == ROOT_RANK(instance)) {
    type = RPL_DAG_MC_ENERGY_TYPE_MAINS;
    instance->mc.obj.energy.energy_est = 0;
  } else {
    type = RPL_DAG_MC_ENERGY_TYPE_BATTERY;
    instance->mc.obj.energy.energy_est = calc_energy;

  }
  instance->mc.obj.energy.flags = type << RPL_DAG_MC_ENERGY_TYPE;
  instance->mc.obj.temperature.gotcha = set;

  PRINTF("ENERGY: node energy = %lu mJ\n",(unsigned long)calc_energy);
  //PRINTF("gotcha: %u\n", (unsigned)instance->mc.obj.temperature.gotcha);

#else

#error "Unsupported RPL_DAG_MC configured. See rpl.h."

#endif /* RPL_DAG_MC */
}
